#include "DRRT.h"
#include <boost/foreach.hpp>
#include "Conversion.h"

#define TOP_TWO 2
#define TOP_FIVE 5
#define A_BIT_PESSIMISTIC 1.1

#define PRINT_STATUS

DRRT::DRRT(vector<Polygon_2> &robot_polygons, int robot_num, Obstacles &obstacles, vector<Conf> &start_confs, vector<Conf> &target_confs, Room &room)
    : m_robot_polygons(robot_polygons), m_robot_num(robot_num), m_obstacles(obstacles), m_start_confs(start_confs), m_target_confs(target_confs), m_room(room),
    m_mr_collision_detector(robot_polygons, robot_num, obstacles)
{
    vector<Polygon_2>::iterator iter_robot = robot_polygons.begin();
    vector<Conf>::iterator iter_start = start_confs.begin();
    vector<Conf>::iterator iter_target = target_confs.begin();
    vector<CollisionDetector*>::const_iterator iter_cd = m_mr_collision_detector.begin();

    for (int i = 0; i < m_robot_num; ++i)
    {
        Prm *prm = new Prm(*iter_robot, m_obstacles, *iter_start, *iter_target, m_room, *iter_cd);
        m_prm.push_back(prm);
        ++iter_robot;
        ++iter_start;
        ++iter_target;
        ++iter_cd;
    }

    m_start = Conversion::to_Point_d(m_start_confs);
    m_target = Conversion::to_Point_d(m_target_confs);
    m_map[m_start] = m_start_confs;
    m_map[m_target] = m_target_confs;
    std::cout << "[dRRT]\t\tInitialized\n";
}

DRRT::~DRRT(void)
{
    for (int i = 0; i < m_robot_num; ++i)
    {
        delete m_prm[i];
    }
    m_prm.clear();
}

bool DRRT::generate_roadmap(int number_vertices, int k_nearest, double eps)
{
    m_eps = eps;
    int index = 1;
    BOOST_FOREACH(Prm* prm, m_prm)
    {
#ifdef PRINT_STATUS
        std::cout << "[dRRT]\t\tBuilding single robot prm (" << index << "\\" << m_robot_num << ")";
#endif
        prm->generate_roadmap(number_vertices, k_nearest, eps);
        if (!prm->has_path())
        {
#ifdef PRINT_STATUS
            std::cout << " - No path...\n";
            std::cout << "[dRRT]\t\tExtending single robot prm (#" << index << ")";
#endif
            prm->generate_roadmap(number_vertices, k_nearest, eps);
            if (!prm->has_path())
            {
#ifdef PRINT_STATUS
                std::cout << " - No path was found... halting!\n";
#endif
                return false;
            }
        }
#ifdef PRINT_STATUS
        std::cout << " - Done! \n";
#endif
        ++index;
    }
    return true;
}

void DRRT::update_path(vector<vector<Conf>> &path, int N, int P)
{
    ConfTree tree_start, tree_target;
    tree_start.graph.add_vertex(m_start);
    tree_start.tree.insert(m_start);
    tree_start.arr.push_back(m_start);
    tree_target.graph.add_vertex(m_target);
    tree_target.tree.insert(m_target);
    tree_target.arr.push_back(m_target);

    list<Point_d> pi;
    bool swap = false;
    int index = 0;
    while (true)
    {
#ifdef PRINT_STATUS
        std::cout << "[dRRT]\t\tLooping #" << index;
        ++index;
        int prev = tree_start.arr.size();
#endif
        expand_tree(tree_start, N);
#ifdef PRINT_STATUS
        std::cout << " (added s#" << tree_start.arr.size() - prev;
        prev = tree_target.arr.size();
#endif
        expand_tree(tree_target, N);
#ifdef PRINT_STATUS
        std::cout << ", t#" << tree_target.arr.size() - prev << ")\t s:" << tree_start.arr.size() << " t:" << tree_target.arr.size() << "\n";
#endif

        if (swap)   //heuristic for swapping the trees when trying to connect them
            pi = connect_trees(tree_target, tree_start, P);
        else
            pi = connect_trees(tree_start, tree_target, P);

        if (!pi.empty())
        {
            if (swap)
            {
                pi.reverse();
            }
            break;
        }
        swap = !swap;
    }
    retrive_path(path, tree_start, tree_target, pi);
}
/*
void print_point(Conf p)
{
    std::cout << "(";
    std::cout << p.x().exact().to_double() << ", " << p.y().exact().to_double();
    std::cout << ")";
}

void print_point(Point_d p)
{
    std::cout << "{";
    bool first = true;
    for(Point_d::Cartesian_const_iterator iter = p.cartesian_begin(); iter != p.cartesian_end(); ++iter)
    {
        if (!first)
        {
            std::cout << ", ";
        }
        std::cout << "(";
        std::cout << *iter << ", ";
        ++iter;
        std::cout << *iter;
        std::cout << ")";
        first = false;
    }
    std::cout << "}";
}
*/
void DRRT::expand_tree(ConfTree &tree, int N)
{
    int max_repetitions = N;
    for (int i = 0; i < N; ++i)
    {
        Point_d q_rand = generate_sample();
        Point_d q_near = tree.tree.nearest_neighbor(q_rand);
        Point_d q_new = direction_oracle(q_near, q_rand);
        /*
        std::cout << "Expanding:\tRand:\t";
        print_point(q_rand);
        std::cout << "\n" << "\t\tq1:\t";
        print_point(q_near);
        std::cout << "\t" << Metrics::distance(q_near, q_rand);
        std::cout << "\n" << "\t\tq2:\t";
        */
        if (q_new.dimension() > 0)
        {
            /*print_point(q_new);
            std::cout << "\t" << Metrics::distance(q_new, q_rand);
            std::cout << "\n";*/
            bool should_add = true;
            BOOST_FOREACH(Point_d& other, tree.arr)
            {
                if (other == q_new)
                {
                    should_add = false;
                    break;
                }
            }
            if (should_add)
            {
                tree.tree.insert(q_new);
                tree.graph.add_vertex(q_new);
                tree.graph.add_edge(q_near, q_new);
                tree.arr.push_back(q_new);
            }
            else
            {
                --max_repetitions;
                if (max_repetitions > 0)
                {
                    --i;
                }
            }
        }
        //std::cout << "\n";
    }
}

list<Point_d> DRRT::connect_trees(ConfTree &tree1, ConfTree &tree2, int P)
{
    for (int i = 0; i < P; ++i)
    {
        Point_d q_rand = generate_sample();
        Point_d q_1 = tree1.tree.nearest_neighbor(q_rand);
        Point_d q_2 = tree2.tree.nearest_neighbor(q_rand);
        /*
        std::cout << "Connecting:\tRand:\t";
        print_point(q_rand);
        std::cout << "\t" << Metrics::distance(q_1, q_2);
        std::cout << "\n" << "\t\tq1:\t";
        print_point(q_1);
        std::cout << "\n" << "\t\tq2:\t";
        print_point(q_2);
        std::cout << "\n\n";

        if (m_mr_collision_detector.local_planner(m_map[q_1], m_map[q_2], 1))
        {
            list<Point_d> pi;
            pi.push_back(q_1);
            pi.push_back(q_2);
            return pi;
        }
        else
        {
            continue;
        }
        */
        DRRTLocalConnector local_connector(m_prm, m_mr_collision_detector, m_eps, TOP_FIVE, A_BIT_PESSIMISTIC);
        list<Point_d> pi = local_connector.connect(m_map[q_1], m_map[q_2], 2);

        if (!pi.empty())
        {
            std::cout << "[dRRT]\t\tPath found!\n";
            pi.push_back(q_2);
            return pi;
        }
    }
    return list<Point_d>();
}

Point_d DRRT::generate_sample()
{
    vector<Conf> points;
    
    BOOST_FOREACH(Prm* prm, m_prm)
    {
        points.push_back(prm->generate_sample());
    }
    Point_d point_d = Conversion::to_Point_d(points);
    m_map[point_d] = points;

    return point_d;
}

void DRRT::retrive_path(vector<vector<Conf>> &path, ConfTree &tree1, ConfTree &tree2, list<Point_d> &pi)
{
    list<Point_d> half_path1;
    list<Point_d> half_path2;

    std::cout << "[dRRT]\t\tRetrieving matched path\n";

    tree1.graph.find_path(m_start, tree1.tree.nearest_neighbor(pi.front()), half_path1);
    tree2.graph.find_path(tree2.tree.nearest_neighbor(pi.back()), m_target, half_path2);

    BOOST_FOREACH(Point_d& point_d, half_path1)
    {
        vector<Conf> point = m_map[point_d];
        path.push_back(point);
    }
    if (!half_path1.empty())
    {
        path.pop_back();    //remove duplicated
    }
    BOOST_FOREACH(Point_d& point_d, pi)
    {
        vector<Conf> point = Conversion::to_vector(point_d);
        path.push_back(point);
    }
    if (!half_path2.empty())
    {
        path.pop_back();    //remove duplicated
    }
    BOOST_FOREACH(Point_d& point_d, half_path2)
    {
        vector<Conf> point = m_map[point_d];
        path.push_back(point);
    }
}

Point_d DRRT::direction_oracle(Point_d &q_near, Point_d &q_rand)
{
    vector<Conf> near = m_map[q_near];
    vector<Conf> rand = Conversion::to_vector(q_rand);

    vector<Conf>::iterator near_iter = near.begin();
    vector<Conf>::iterator rand_iter = rand.begin();

    vector<Conf> new_samples;

    BOOST_FOREACH(Prm* prm, m_prm)
    {
        Conf c = prm->get_neighbor(*near_iter, *rand_iter);
        /*
        std::cout << "Searching:\tRand:\t";
        print_point(q_rand);
        std::cout << "\n" << "\t\tq1:\t";
        print_point(*near_iter);
        std::cout << "\t" << Metrics::distance(*near_iter, *rand_iter);
        std::cout << "\n" << "\t\tq2:\t";
        print_point(c);
        std::cout << "\t" << Metrics::distance(c, *rand_iter);
        std::cout << "\n\n";
        */
        new_samples.push_back(c);
        ++near_iter;
        ++rand_iter;
    }
    
    if (!m_mr_collision_detector.local_planner(near, new_samples, m_eps))
    {
        return Point_d();
    }
    Point_d p = Conversion::to_Point_d(new_samples);
    m_map[p] = new_samples;
    return p;
}
